﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Collections;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.Diagnostics;
using System.IO;

using RobotCtrl;
using RobotView;

namespace Test_WinCE_RobotDrive
{
    public partial class Form1 : Form
    {
        private Robot robot;
        private World world;
        private Thread controlThread;
        private bool isRunning;
        private List<Track> path; //Path built via gui 
        private State lastState;  //Last state we were in
        private double speed = 0.5;     //Speed for next movement
        private double accel;     //acceleration
        private double currentPositionX;
        private double currentPositionY;
        private RectangleF area;
        RunMode runmode = RunMode.VIRTUAL;
        Thread pathThread;

        private enum SwitchMeanings : int
        {
            SHOW = 0,
            INSPECTION = 1,
            NUTTIN = 2,
            SPEED = 3
        }

        private enum State
        {
            SHOWING,
            INSPECTING,
            SETTING_SPEED_1,
            SETTING_SPEED_0_5,
            RESETTING,
            SHUTDOWN
        }

        public Form1()
        {
            InitializeComponent();
            
            initRobot();
            path = new List<Track>();
            area = new RectangleF(0.0f, -3.0f, 6.0f, 6.0f);
                        
            if (runmode == RunMode.VIRTUAL)
            {
                String obstacleImagePath;
                if (Config.IsWinCE)
                {
                    obstacleImagePath = @"CompactFlash\ObstacleImageInv.bmp";
                }
                else
                {
                    obstacleImagePath = @"ObstacleImageInv.bmp";
                }

                Bitmap obstacleImage = new Bitmap(obstacleImagePath);
                World.ObstacleMap = new ObstacleMap(obstacleImage, area);

            }
            else if (runmode == RunMode.REAL)
            {
                this.controlThread = new Thread(controlMotion);
                this.Closing += new CancelEventHandler(Form1_Closing);
                this.controlThread.Start();
            }

        }

        private void initRobot()
        {
            robot = new Robot(runmode);
            robot.RelativeRadarPosition = new DrivePosition(0.25, 0.125, +70.0);
            world = new World(robot, 10, 10, 2, 5);
            this.accel = 0.3;
            this.worldView.World = world;
        }

        private void Form1_Closing(object sender, CancelEventArgs e)
        {
            //Due to WinCE limitations
            this.controlThread.Abort();
        }

        /// <summary>
        /// Runs a specifi program (track) consisting of multiple path parts
        /// </summary>
        private void runProgramInPath()
        {
            //robot.Console.Lamps[1] = true;
            //robot.Drive.RunLine(1, 1, 1);
            //robot.Drive.Stop();
            //robot.Drive.RunTurn(180, 1, 1);
            //robot.Drive.RunArcLeft(1, 360, 10, 1);
            //Thread.Sleep(4000);
            this.lastState = State.SHOWING;
            robot.Drive.Power = true;
            
            //drive Path that has been built with trackbuilder
            {
                foreach(Track t in path)
                {
                    if (t is TrackArcLeft)
                    {
                        TrackArcLeft tAL = (TrackArcLeft)t;
                        robot.Drive.RunArcLeft(tAL.radius, tAL.angle, tAL.speed, tAL.acceleration);
                    }
                    if (t is TrackArcRight)
                    {
                        TrackArcRight tAR = (TrackArcRight)t;
                        robot.Drive.RunArcRight(tAR.radius, tAR.angle, tAR.speed, tAR.acceleration);
                    }
                    if (t is TrackLine)
                    {
                        TrackLine tL = (TrackLine)t;
                        robot.Drive.RunLine(tL.length, tL.speed, tL.acceleration);
                    }
                    //Hack
                    if (t is TrackRadar)
                    {
                        robot.Drive.RunLine(t.length, t.speed, t.acceleration);
                    }

                    if (t is TrackPause)
                    {
                        TrackPause tP = (TrackPause)t;
                        robot.Drive.RunPause(tP.pause);
                    }
                    if (t is TrackTurn)
                    {
                        TrackTurn tT = (TrackTurn)t;
                        robot.Drive.RunTurn(tT.angle, tT.speed, tT.acceleration);
                    }
                    //TODO
                    //Wait for robot to finish this part of the path
                    while (!robot.Drive.Done) 
                    { 
                        Thread.Sleep(5); //give some time to other tasks
                    }
                }
                path.Clear();
            }
            
            robot.Drive.Power = false;
            //this.isRunning = false;
        }

        /// <summary>
        /// Controls the start and stop of a specific program
        /// by monitoring the switch status
        /// Switch 0: Show()
        /// Switch 1: Inspection()
        /// Switch 2: dog
        /// Switch 3: Speed
        /// Stati Octal: 1 = Show(), 2=inspection, 8 = speed, 
        ///     (Alle anderen Kombinationen sind ungültig)
        /// </summary>
        private void controlMotion()
        {
            int switchStateValue = 0;
            while (true)
            {
                switchStateValue = this.getSwitchStateValue();
                switch (switchStateValue)
                {
                    case 0:
                        if (this.lastState != State.RESETTING)
                        {
                            this.reset();
                            //this.lastState = State.RESETTING;
                        }
                        break;
                    case 9: 
                    case 17:
                        if (this.lastState != State.SHOWING)
                        {
                            this.fillPathWithShowProgram();
                            this.runProgramInPath();
                            this.lastState = State.SHOWING;
                        }
                        break;
                    case 10:
                    case 18:
                        if (this.lastState != State.INSPECTING)
                        {
                            this.runInspectionNew();
                            this.lastState = State.INSPECTING;
                        }
                        break;
                    case 8:
                        if (this.lastState != State.SETTING_SPEED_1)
                        {
                            //this.speed = 1.0;
                            this.speed = 0.8;
                            this.lastState = State.SETTING_SPEED_1;
                        }
                        break;
                    case 16:
                        if (this.lastState != State.SETTING_SPEED_0_5)
                        {
                            this.speed = 0.5;
                            this.lastState = State.SETTING_SPEED_0_5;
                        }
                        break;
                    case 4:
                        {
                            //this.robot.Drive.Power = false;
                        }
                        break;

                }
                switchStateValue = 0; //Reset state for next test
                Thread.Sleep(10); //Give time to other threads, f.e. painter
            }

        }

        /// <summary>
        /// Fills the show program with an 8
        /// </summary>
        private void fillPathWithShowProgram()
        {
            //drive an 8
            double l = 1;
            this.path.Add(new TrackLine(l, this.speed, this.accel));
            this.path.Add(new TrackTurn(-90, this.speed, this.accel));
            this.path.Add(new TrackArcLeft(0.5, 180, this.speed, this.accel));
            this.path.Add(new TrackArcRight(0.5, 180, this.speed, this.accel));
            this.path.Add(new TrackTurn(90, this.speed, this.accel));
            this.path.Add(new TrackLine(-3*l, this.speed, this.accel));
        }

        private void reset()
        {

            robot.Drive.Power = false;
            this.lastState = State.RESETTING;
        }

        /// <summary>
        /// Returns the switch state in defined format
        /// </summary>
        /// <returns></returns>
        private int getSwitchStateValue()
        {
            int state = 0;
            if (robot.Console.Switches[(int)SwitchMeanings.SHOW])
            {
                state = state + 1;
            }
            if (robot.Console.Switches[(int)SwitchMeanings.INSPECTION])
            {
                state = state + 2;
            }
            if (robot.Console.Switches[(int)SwitchMeanings.NUTTIN])
            {
                state = state + 4;
            }
            if (robot.Console.Switches[(int)SwitchMeanings.SPEED])
            {
                state = state + 8 ;
            }
            else
            {
                state = state + 16;
            }
            return state;
        }

        #region inspection

        /// <summary>
        /// Main Entry point for inspection part
        /// </summary>
        private void runInspection()
        {
            this.isRunning = true;

            //all values in meter
            double distanceThresholdMin = 0.4;
            double distanceThresholdMax = 0.6;
            double stepSize = 0.1;
            double radar_angle = 70;
            bool isRecording = false;

            //Init Block
            //----------------
            this.path.Add(new TrackTurn(-90, this.speed, this.accel));
            this.runProgramInPath();

            //Position vars
            DoublePoint oldPos = new DoublePoint(robot.Position.X, robot.Position.Y);
            double distance_hypo = robot.Radar.Distance;
            double vertDistanceToRobot_old = distance_hypo * Math.Sin(radar_angle);

            this.path.Add(new TrackLine(stepSize, this.speed, this.accel));
            this.runProgramInPath();

            DoublePoint newPos = new DoublePoint(robot.Position.X, robot.Position.Y);

            //End Init
            //----------------

            //List<double> hypos = new List<double>();
            //TODO: ADD that robot stops after one turn --> define an area
            while (isRunning)
            {
                if (isRecording == false && !checkInStartingArea(newPos))
                {
                    isRecording = true;
                }
                else if (isRecording == true && checkInStartingArea(newPos))
                {
                    isRunning = false;
                }


                //TEST
                distance_hypo = 0;
                for (int i = 0; i < 4; i++)
                {
                    distance_hypo += robot.Radar.Distance;
                    //hypos.Add(distance_hypo);
                }
                distance_hypo /= 4;
                
                double vertDistanceToRobot_new = distance_hypo * Math.Sin(radar_angle*Math.PI/180);

                //Get Correction angle
                double distRadarDelta = vertDistanceToRobot_new - vertDistanceToRobot_old;
                //Robotdistance ist eine hypotenuse
                double distRoboDelta = Math.Sqrt(Math.Pow(Math.Abs(newPos.X - oldPos.X), 2)
                                        + Math.Pow(Math.Abs(newPos.Y - oldPos.Y), 2));
                double correction_angle = Math.Abs(Math.Atan(distRadarDelta / distRoboDelta)
                                            * 180 / Math.PI);
                //Links korrigierien falls distanz >255 (max-val)
                if (distRadarDelta == 0)
                {
                    this.path.Add(new TrackArcLeft(0.5, 30, this.speed, this.accel));
                }
                else if (vertDistanceToRobot_new > distanceThresholdMin && vertDistanceToRobot_new < distanceThresholdMax)
                {
                    //move straight
                    this.path.Add(new TrackLine(stepSize, this.speed, this.accel));
                }
                else if (vertDistanceToRobot_new > distanceThresholdMin)
                {
                    //move left
                   //this.path.Add(new TrackTurn(correction_angle/4, this.speed, this.accel));
                   this.path.Add(new TrackArcLeft(0.5, correction_angle/4, this.speed, this.accel));
                }
                else if (vertDistanceToRobot_new < distanceThresholdMax)
                {
                    //move right
                    //this.path.Add(new TrackTurn(-correction_angle/4, this.speed, this.accel));
                    this.path.Add(new TrackArcRight(0.5, correction_angle/4, this.speed, this.accel));
                }
                //this.path.Add(new TrackLine(stepSize, this.speed, this.accel));
                

                oldPos = newPos;
                this.runProgramInPath();
                //Bug robot.Position.X
                double a  = robot.Position.X;
                double b = robot.Position.Y;
                newPos.X = a;
                newPos.Y = b;
                vertDistanceToRobot_old = vertDistanceToRobot_new;
            }
        }

        private void runInspectionNew()
        {
            robot.Drive.Power = true;

            this.isRunning = true;

            //all values in meter
            bool isRecording = false;

            // set current position
            currentPositionX = robot.Drive.Position.X;
            currentPositionY = robot.Drive.Position.Y;

            //Init Block
            //----------------
            this.robot.Drive.RunTurn(-90, this.speed, this.accel);
            while (!robot.Drive.Done)
            {
                Thread.Sleep(5); //give some time to other tasks
            }
            this.robot.Drive.RunLine(0.1, this.speed, this.accel);
            
            while (!robot.Drive.Done)
            {
                Thread.Sleep(5); //give some time to other tasks
            }
            
            //Start by going straight
            TrackRadar runner = new TrackRadar(this.speed, this.accel);
            this.robot.Drive.RunRadar(runner);

            //End Init
            //----------------
            //Check if we turned once, the rest is in the drive class
            while (isRunning && !robot.Drive.Done)
            {
                
                DoublePoint newPos = new DoublePoint(robot.Position.X, robot.Position.Y);
                if (isRecording == false && !checkInStartingArea(newPos))
                {
                    isRecording = true;
                }
                else if (isRecording == true && checkInStartingArea(newPos))
                {
                    isRunning = false;
                    runner.stop = true;
                }
                //Zeit für andere Tasks
                Thread.Sleep(15);

                #region auskommentiert neu
                /*
                //TEST
                distance_hypo = 0;
                for (int i = 0; i < 4; i++)
                {
                    distance_hypo += robot.Radar.Distance;
                    //hypos.Add(distance_hypo);
                }
                distance_hypo /= 4;

                double vertDistanceToRobot_new = distance_hypo * Math.Sin(radar_angle*Math.PI/180);

                //Get Correction angle
                double distRadarDelta = vertDistanceToRobot_new - vertDistanceToRobot_old;
                //Robotdistance ist eine hypotenuse
                double distRoboDelta = Math.Sqrt(Math.Pow(Math.Abs(newPos.X - oldPos.X), 2)
                                        + Math.Pow(Math.Abs(newPos.Y - oldPos.Y), 2));
                double correction_angle = Math.Abs(Math.Atan(distRadarDelta / distRoboDelta)
                                            * 180 / Math.PI);
               
                //Abstand zu klein
                if(Double.IsNaN(correction_angle))
                {
                    //throw new SystemException();
                }
                //Links korrigierien falls distanz >255 (max-val)
                else if (distRadarDelta == 0 || vertDistanceToRobot_new > distanceThresholdMin)
                {
                    //move left
                   //this.path.Add(new TrackTurn(correction_angle/4, this.speed, this.accel));
                    runner.angle = correction_angle;
                   //this.path.Add(new TrackArcLeft(0.5, correction_angle/4, this.speed, this.accel));
                }
                else if (vertDistanceToRobot_new < distanceThresholdMax)
                {
                    //move right
                    //this.path.Add(new TrackTurn(-correction_angle/4, this.speed, this.accel));
                    //this.path.Add(new TrackArcRight(0.5, correction_angle/4, this.speed, this.accel));
                    runner.angle = -correction_angle;
                }
                //this.path.Add(new TrackLine(stepSize, this.speed, this.accel));
                

                oldPos = newPos;
                //this.runProgramInPath();
                //Bug robot.Position.X

                Thread.Sleep(1000);

                double a  = robot.Position.X;
                double b = robot.Position.Y;
                newPos.X = a;
                newPos.Y = b;
                vertDistanceToRobot_old = vertDistanceToRobot_new;
                */
                #endregion
            }

            //robot.Drive.Power = false;


        }


        private bool checkInStartingArea(DoublePoint actualPosition)
        {
            double r = 0.5;
            double hypo = Math.Sqrt(Math.Pow(Math.Abs(actualPosition.X - currentPositionX), 2)
                                        + Math.Pow(Math.Abs(actualPosition.Y - currentPositionY), 2));
            if (hypo > r)
            {
                //ausserhalb vom feld
                return false;
            }
            else
            {
                return true;
            }
        }


        #endregion



        #region Pathbuilder Event Handlers and layout

        private void btnAddLine_Click(object sender, EventArgs e)
        {
            double length = Double.Parse(this.txtLengthLine.Text);
            double v = Double.Parse(this.txtSpeedLine.Text);
            double a = Double.Parse(this.txtAccelLine.Text);
            this.path.Add(new TrackLine(length, v, a));
        }

        private void btnAddLeftArc_Click(object sender, EventArgs e)
        {
            double r = Double.Parse(this.txtRadiusLeftArc.Text);
            double angle = Double.Parse(this.txtAngleLeftArc.Text);
            double v = Double.Parse(this.txtSpeedLeftArc.Text);
            double a = Double.Parse(this.txtAccelLeftArc.Text);
            this.path.Add(new TrackArcLeft(r,angle,v,a));
        }

        private void btnAddRightArc_Click(object sender, EventArgs e)
        {
            double r = Double.Parse(this.txtRadiusRightArc.Text);
            double angle = Double.Parse(this.txtAngleRightArc.Text);
            double v = Double.Parse(this.txtSpeedRightArc.Text);
            double a = Double.Parse(this.txtAccelRightArc.Text);
            this.path.Add(new TrackArcRight(r,angle,v,a));
        }

        private void btnAddTurn_Click(object sender, EventArgs e)
        {
            double angle = Double.Parse(this.txtAngleTurn.Text);
            double v = Double.Parse(this.txtSpeedTurn.Text);
            double a = Double.Parse(this.txtAccelTurn.Text);
            this.path.Add(new TrackTurn(angle,v,a));
        }

        #endregion

        private void button1_Click(object sender, EventArgs e)
        {
            
            this.robot.Drive.Power = true;
            this.robot.Drive.Reset();
            this.fillPathWithShowProgram();
            pathThread = new Thread(delegate() { this.runProgramInPath(); });
            pathThread.Start();
            
            
        }

        private void button2_Click(object sender, EventArgs e)
        {
            this.robot.Drive.Reset();
            new Thread(delegate() { this.runInspectionNew(); }).Start();
        }

        private void textBox1_TextChanged(object sender, EventArgs e)
        {
            this.speed = Double.Parse(textBox1.Text);
        }

        private void button3_Click(object sender, EventArgs e)
        {
            try
            {
                if (pathThread != null)
                {
                    pathThread.Abort();
                }
            }
            catch (ThreadAbortException ex)
            {
                Console.WriteLine(ex.StackTrace);
            }

            this.robot.Close();
            this.initRobot();
        }

        /*
        private void controlMotionOld()
        {
            bool lastState = false;
            bool switchStateShow;
            while (true)
            {
                switchStateShow = robot.Console.Switches[3];
                if (switchStateShow != lastState //&& switchState==true 
                   && this.isRunning == false && path.Count > 0)
                {
                    runShow();
                    lastState = robot.Console.Switches[3];
                }

            }
        }*/      

    }
}